#ifndef _PLANNER_MANAGER_H_
#define _PLANNER_MANAGER_H_

#include <stdlib.h>

#include <bspline_opt/bspline_optimizer.h>
#include <bspline_opt/uniform_bspline.h>
#include <ego_planner/DataDisp.h>
#include <plan_env/grid_map.h>
#include <plan_manage/plan_container.hpp>
#include <ros/ros.h>
#include <traj_utils/planning_visualization.h>

namespace ego_planner
{

    // Fast Planner Manager
    // Key algorithms of mapping and planning are called

    class EGOPlannerManager
    {
        // SECTION stable
    public:
        EGOPlannerManager();
        ~EGOPlannerManager();

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        /* main planning interface */
        bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
                           Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj);
        bool EmergencyStop(Eigen::Vector3d stop_pos);
        bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
                            const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
        bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
                                     const std::vector<Eigen::Vector3d> &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);

        void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL);

        PlanParameters pp_;
        LocalTrajData local_data_;
        GlobalTrajData global_data_;
        GridMap::Ptr grid_map_;

    private:
        /* main planning algorithms & modules */
        PlanningVisualization::Ptr visualization_;

        BsplineOptimizer::Ptr bspline_optimizer_rebound_;

        int continous_failures_count_{0};

        void updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now);

        void reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt,
                            double &time_inc);

        bool refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);

        // !SECTION stable

        // SECTION developing

    public:
        typedef unique_ptr<EGOPlannerManager> Ptr;

        // !SECTION
    };
} // namespace ego_planner

#endif